#include "robot_manager.h"

void RobotManager::publishJointStatesLoop()
{
    ros::Rate rate(CONTROL_FREQ);
    std::vector<double> q(7, 0.0), qd(7, 0.0), qdd(7, 0.0), torque;
    std::vector<double> D = {1.0, 1.2, 1.1, 1.1, 1.3, 1.3, 1.0};

    std::vector<double> collision_force_threshold;
    nh_.getParam("collision_force_threshold", collision_force_threshold);
    while (running_ && ros::ok())
    {
        if (!is_simulation_)
        {
            pthread_mutex_lock(&zoros_mutex_);
            joint_state_msg_.header.stamp = ros::Time::now();

            for (int i = 0; i < 7; i++)
            {
                double position = motors_[i].Get_Position();
                if(position > 3 || position < -3)
                {
                    static unsigned int j;
                    if(j == 100)
                    {
                        ROS_WARN("joint [%d] position: %.4f out of range!", i + 1, position);
                        j = 0;
                    }
                    j++;
                }
                dm_->refresh_motor_status(motors_[i]);
                joint_state_msg_.position[i] = position;
                joint_state_msg_.velocity[i] = motors_[i].Get_Velocity();
                joint_state_msg_.effort[i] = motors_[i].Get_tau();
            }
#if 0
//! TODO: 碰撞检测逻辑还有问题
            q = joint_state_msg_.position;
            robot_model_->inverseDynamics(q, qd, qdd, torque);
            if(teach_mode_ == false)
            {
                for(int i = 0; i < 7; i++)
                {
                    if(fabs(joint_state_msg_.velocity[i]) > 0.02 && fabs(torque[i] * D[i] - joint_state_msg_.effort[i]) > collision_force_threshold[i])
                    {
                        ROS_WARN("Joint [%d] torque over limit, maybe collision!", i + 1);
                        teach_mode_ = true;
                        break;
                    }
                }
            }
            else
            {
                for(int i = 0; i < 7; i++)
                {
                    dm_->control_mit(motors_[i], 0.0, 0.0, 0.0, 0.0, torque[i] * D[i]);
                }
            }
#endif
            joint_pub_.publish(joint_state_msg_);
            pthread_mutex_unlock(&zoros_mutex_);
        }

        rate.sleep();
    }
}